#include "test.h"


extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

extern TIM_HandleTypeDef htim4;

extern UART_HandleTypeDef huart4;
extern UART_HandleTypeDef huart5;
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
extern UART_HandleTypeDef huart6;

extern osMessageQId CommandQueueHandle;

uint8_t getValueFromQueue(void);


void TEST_UART(void)
{
  printf("UART Test Start ...\r\n");
  uint8_t  tx_tmp;
  uint8_t rx_tmp;
  uint8_t num ;
  num = 10;
  while(num)
  {
      tx_tmp = 0x55;
      rx_tmp  = 0;
      HAL_UART_Transmit(&huart2,&tx_tmp, 1, 10);
      HAL_UART_Receive(&huart3, &rx_tmp, 1, 10);
      if(tx_tmp != rx_tmp)
      {   
          PRINT(RED,"UART2 -> UART3 ERROR\r\n");
          break;
      }
      tx_tmp = 0x55;
      rx_tmp  = 0;
      HAL_UART_Transmit(&huart3,&tx_tmp, 1, 10);
      HAL_UART_Receive(&huart2, &rx_tmp, 1, 10);
      if(tx_tmp != rx_tmp)
      {   
          PRINT(RED,"UART2 <- UART3 ERROR\r\n");
          break;
      }
      num --;   
      if(num == 0)
      {
        PRINT(GREEN,"UART2 <-> UART3 PASS\r\n");
      }
  }
  num = 10;
  while(num)
  {
      tx_tmp = 0x55;
      rx_tmp  = 0;
      HAL_UART_Transmit(&huart4,&tx_tmp, 1, 10);
      HAL_UART_Receive(&huart5, &rx_tmp, 1, 10);
      if(tx_tmp != rx_tmp)
      {   
          PRINT(RED,"UART4 -> UART5 ERROR\r\n");
          break;
      }
      tx_tmp = 0x55;
      rx_tmp  = 0;
      HAL_UART_Transmit(&huart5,&tx_tmp, 1, 10);
      HAL_UART_Receive(&huart4, &rx_tmp, 1, 10);
      if(tx_tmp != rx_tmp)
      {   
          PRINT(RED,"UART4 <- UART5 ERROR\r\n");
          break;
      }
      num --;   
      if(num == 0)
      {
          PRINT(GREEN,"UART4 <-> UART5 PASS\r\n");
      }
  }
}



void TEST_CAN(void)
{
 printf("CAN Test Start ...\r\n");
 canTestConfing();
 canTestAction();
}



void TEST_IO(void)
{
    printf("IO Test Start ...\r\n");
    printf("Enter any key to exit IO test...");
    while(1)
    {
        if(HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_13) == 0)
            LED_YellowOn();
        else
            LED_YellowOff();
        
        if(HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_4) == 0)
            LED_GreenOn();
        else
            LED_GreenOff();
        
        if(HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_5) == 0)
            LED_BlueOn();
        else
            LED_BlueOff();
        
        if(HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_6) == 0)
            BUZZER_On();
        else
            BUZZER_Off();
        osEvent event;
        event =  osMessageGet(CommandQueueHandle,1);
        if(event.status == osEventMessage)
        {
            BUZZER_Off();
            LED_BlueOff();
            LED_YellowOff();
            LED_GreenOff();
            BUZZER_Period(1);
            break;
        }
    }
    printf("\rIs the IO test successful? [Y/n]");
    uint8_t tmp =  getValueFromQueue();
    printf(" %c",tmp);    
    if(tmp  == 'n')
        tmp = 'N';
    if(tmp == 'N')
    {
       BUZZER_Period(2); 
       PRINT(RED,"\rIO TEST ERROR                         \r\n");
    }
    else
    {
       BUZZER_Period(2); 
       PRINT(GREEN,"\rIO TEST PASS                         \r\n");   
    }
}



void TEST_ADC(void)
{
  printf("ADC Test Start ...\r\n");
  ADS111X_Init();
  BUZZER_Period(2);
 // MOVEUP(1);
  printf("\rEnter any key to exit ADC test...  ");
  MOVEUP(1);
  printf("\33[?25l");
  printf("\r                                                ");
  while(1)
  {
      printf("\rThe voltage is %7.4fV",read_ADS111X());
      osEvent event;
      event =  osMessageGet(CommandQueueHandle,1);
      if(event.status == osEventMessage)
      {
          BUZZER_Period(1);
          break;
      }
  }
  printf("\r\n                                       ");
  MOVEUP(1);
  printf("\rIs the ADC test successful? [Y/n]");
  uint8_t tmp =  getValueFromQueue();
  printf(" %c",tmp);    
  if(tmp  == 'n')
      tmp = 'N';
  if(tmp == 'N')
  {
      BUZZER_Period(2); 
      PRINT(RED,"\rADC TEST ERROR                         \r\n");
  }
  else
  {
      BUZZER_Period(2); 
      PRINT(GREEN,"\rADC TEST PASS                         \r\n");   
  }
}



void TEST_PWM(void)
{
    printf("PWM Test Start ...\r\n");
    printf("Enter any key to start PWM test...");
    InitPWM(TIM_CHANNEL_1);
    InitPWM(TIM_CHANNEL_2);
    InitPWM(TIM_CHANNEL_3);
    InitPWM(TIM_CHANNEL_4);
    osMessageGet(CommandQueueHandle,osWaitForever);
    BUZZER_Period(1);    
    SetPWM(TIM_CHANNEL_1,0);
    SetPWM(TIM_CHANNEL_2,0);
    SetPWM(TIM_CHANNEL_3,0);
    SetPWM(TIM_CHANNEL_4,0);
    HAL_Delay(500);
    SetPWM(TIM_CHANNEL_1,500);
    SetPWM(TIM_CHANNEL_2,500);
    SetPWM(TIM_CHANNEL_3,500);
    SetPWM(TIM_CHANNEL_4,500);
    HAL_Delay(500);    
    SetPWM(TIM_CHANNEL_1,-500);
    SetPWM(TIM_CHANNEL_2,-500);
    SetPWM(TIM_CHANNEL_3,-500);
    SetPWM(TIM_CHANNEL_4,-500);
    HAL_Delay(1000);
    SetPWM(TIM_CHANNEL_1,0);
    SetPWM(TIM_CHANNEL_2,0);
    SetPWM(TIM_CHANNEL_3,0);
    SetPWM(TIM_CHANNEL_4,0);
    HAL_Delay(500);
    printf("\rIs the PWM test successful? [Y/n]                                               ");
    uint8_t tmp =  getValueFromQueue();
    printf("\rIs the PWM test successful? [Y/n]");
    printf(" %c",tmp);    
    if(tmp  == 'n')
        tmp = 'N';
    if(tmp == 'N')
    {
        BUZZER_Period(2); 
        PRINT(RED,"\rPWM TEST ERROR                         \r\n");
    }
    else
    {
        BUZZER_Period(2); 
        PRINT(GREEN,"\rPWM TEST PASS                         \r\n");   
    }

}



void TEST_IMU(void)
{
    printf("IMU Test Start ...\r\n");
    IMU_Config();
//    printf("MPU6050 = %d\n",MPU6050_TestConnection());
//    MPU6050_SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
//    HAL_Delay(2000);
//    MPU6050_SetFullScaleGyroRange(0);
//    MPU6050_SetFullScaleAccelRange(0);
//    while(1)
//    {
//      int16_t temp[7];
//      MPU6050_GetRawAccelGyro(temp);
//      int i = 0;
//      for(i = 0; i < 7; i++)
//      {
//        printf("temp[%d] = %d ", i, temp[i]);
//      }
//      printf("\r\n");
//    }
    HAL_Delay(300);
    MOVEUP(1);
    printf("Enter any key to exit IMU test..."); 
    MOVEUP(1);
    printf("\rAcceleration: x =  0.00g, y =  0.00g, z =  0.00g. Angular: z =   0");
    printf("\33[?25l");
    while(1)
    {   
        int16_t x = 0;
        int16_t y = 0;
        int16_t z = 0;
        int16_t a_z = 0;
        Get_Acceleration(&x,&y,&z);
        Get_Gyroscope(&a_z);
       // printf("\rAcceleration: x = %05.2fg, y = %05.2fg, z = %05.2fg. Angular acceleration: y = %04d",x*2.0/2048,y*2.0/2048,z*2.0/2048,a_z);
        printf("\r");
        MOVERIGHT(18);
        printf("%05.2f",x*2.0/2048); 
        MOVERIGHT(7);
        printf("%05.2f",y*2.0/2048);
        MOVERIGHT(7);
        printf("%05.2f",z*2.0/2048);
        MOVERIGHT(16);
        printf("%04d",a_z);
        printf("    ");
        HAL_Delay(20);
        
        osEvent event;
        event =  osMessageGet(CommandQueueHandle,1);
        if(event.status == osEventMessage)
        {
            BUZZER_Period(1);
            break;
        }
    }
    printf("\r\n                                    \r");
    MOVEUP(1);
    printf("Is the IMU test successful? [Y/n]                                               ");
    uint8_t tmp =  getValueFromQueue();
    printf("\rIs the IMU test successful? [Y/n]");
    printf(" %c",tmp);    
    if(tmp  == 'n')
        tmp = 'N';
    if(tmp == 'N')
    {
        BUZZER_Period(2); 
        PRINT(RED,"\rIMU TEST ERROR                         \r\n");
    }
    else
    {
        BUZZER_Period(2); 
        PRINT(GREEN,"\rIMU TEST PASS                         \r\n");   
    }
    
}


void TEST_OBSTACLE(void)
{
    printf("OBSTACLE Test Start ...\r\n");
    printf("Select area( 1 ~ F ):");
    uint8_t area =  getValueFromQueue();
    printf(" %c\r\n",area);
    if( area >= '1' && area <= '9')
    {
        PBS03JN_SetArea(1,area - 0x30);
        PBS03JN_SetArea(2,area - 0x30);   
    }
    else if( area >= 'A' && area <= 'F')
    {
        PBS03JN_SetArea(1,area - 55);
        PBS03JN_SetArea(2,area - 55);
    }
    else if( area >= 'a' && area <= 'f')
    {
        PBS03JN_SetArea(1,area - 87);
        PBS03JN_SetArea(2,area - 87);
    }
    else
    {
        printf("Invalid parameters\r\n");
        return;
    }
    printf("Enter any key to exit OBSTACLE test..."); 

    MOVEUP(1);
    printf("\33[?25l");
    while(1)
    {
        printf("\rObstacle status-- channel1:%2d, channel2:%2d",PBS03JN_GetOutput(1),PBS03JN_GetOutput(2));
        osEvent event;
        event =  osMessageGet(CommandQueueHandle,1);
        if(event.status == osEventMessage)
        {
            BUZZER_Period(1);
            break;
        }
    }
    printf("\r\n                                    \r");
    MOVEUP(1);
    printf("Is the OBSTACLE test successful? [Y/n]                                               ");
    uint8_t tmp =  getValueFromQueue();
    printf("\rIs the OBSTACLE test successful? [Y/n]");
    printf(" %c",tmp);    
    if(tmp  == 'n')
        tmp = 'N';
    if(tmp == 'N')
    {
        BUZZER_Period(2); 
        PRINT(RED,"\rOBSTACLE TEST ERROR                         \r\n");
    }
    else
    {
        BUZZER_Period(2); 
        PRINT(GREEN,"\rOBSTACLE TEST PASS                         \r\n");   
    }
}

void TEST_WAVE(void)
{
    printf("Wave Test Start ...\r\n");
    printf("Enter any key to exit wave test..."); 
    uint8_t ch = 0x55;
    canTestConfing();
    while(1)
    {
        HAL_CAN_Transmit(&hcan1, 10);
        HAL_UART_Transmit(&huart5, (uint8_t *)&ch, 1, 0xFFFF);
        HAL_UART_Transmit(&huart4, (uint8_t *)&ch, 1, 0xFFFF);
        HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 0xFFFF);
        HAL_UART_Transmit(&huart3, (uint8_t *)&ch, 1, 0xFFFF);
        HAL_CAN_Transmit(&hcan2, 10);
        osEvent event;
        event =  osMessageGet(CommandQueueHandle,1);
        if(event.status == osEventMessage)
        {
            BUZZER_Period(1);
            break;
        }
    }
}

